﻿using System;
using System.Collections.Generic;

namespace Utils
{
    public class VectorDouble
    {
        public int Dimension { get; private set; }
        private List<double> _vector;

        public VectorDouble(int dimension)
        {
            Dimension = dimension;
            _vector = new List<double>();

            for (int i = 0; i < Dimension; i++)
            {
                _vector.Add(0);
            }
        }

        public double Dot(VectorDouble other)
        {
            if (other.Dimension != Dimension) throw new Exception("Dim miss-match");

            double ret = 0;
            for (int i = 0; i < Dimension; i++)
            {
                ret += this[i] * other[i];
            }
            return ret;
        }

        public VectorDouble Cross(VectorDouble other)
        {
            if (other.Dimension != Dimension || Dimension != 3) throw new Exception("Dim miss-match");

            var ret = new VectorDouble(Dimension);
            var mat = new MatrixDouble(Dimension, Dimension);

            var import = new List<double>(other.ToArray());
            import.AddRange(ToArray());

            if (Dimension == 3)
                import.AddRange(new List<double> { 0, 0, 0 });

            mat.Import(import);
            mat = mat.Rotate();

            for (int i = 0; i < Dimension; i++)
            {
                ret[i] = (i % 2 == 0 ? 1d : -1d) * mat.Cofactor(0, i);
            }

            return ret;
        }

        public VectorDouble Normalize()
        {
            var ret = new VectorDouble(Dimension);
            double total = 0;
            foreach (var d in _vector)
            {
                total += d * d;
            }

            total = Math.Sqrt(total);

            for (int i = 0; i < Dimension; i++)
            {
                ret[i] = this[i] / total;
            }

            return ret;
        }

        public VectorDouble Add(VectorDouble other)
        {
            if (other.Dimension != Dimension) throw new Exception("Dim miss-match");

            var ret = new VectorDouble(Dimension);
            for (int i = 0; i < Dimension; i++)
            {
                ret[i] = this[i] + other[i];
            }

            return ret;
        }

        public VectorDouble Subtract(VectorDouble other)
        {
            if (other.Dimension != Dimension) throw new Exception("Dim miss-match");

            var ret = new VectorDouble(Dimension);
            for (int i = 0; i < Dimension; i++)
            {
                ret[i] = this[i] - other[i];
            }

            return ret;
        }

        public VectorDouble Multiply(VectorDouble other)
        {
            if (other.Dimension != Dimension) throw new Exception("Dim miss-match");

            var ret = new VectorDouble(Dimension);
            for (int i = 0; i < Dimension; i++)
            {
                ret[i] = this[i] * other[i];
            }

            return ret;
        }

        public VectorDouble Multiply(double value)
        {
            var ret = new VectorDouble(Dimension);
            for (int i = 0; i < Dimension; i++)
            {
                ret[i] = this[i] * value;
            }

            return ret;
        }

        public void Import(List<double> import)
        {
            _vector = new List<double>(import);
        }

        public double this[int i]
        {
            get { return _vector[i]; }
            set { _vector[i] = value; }
        }

        public List<double> ToArray()
        {
            return new List<double>(_vector);
        }
    }
    public class MatrixDouble
    {
        /// <summary>
        /// A collection of Column vectors
        /// </summary>
        private readonly List<List<double>> _matrix;

        public MatrixDouble(int rows, int cols, List<double> import)
        {
            Rows = rows;
            Columns = cols;

            _matrix = new List<List<double>>(rows);

            for (int i = 0; i < rows; i++)
            {
                var vector = new List<double>(cols);
                for (int j = 0; j < cols; j++)
                {
                    vector.Add(0);
                }
                _matrix.Add(vector);
            }
            Import(import);
        }

        public MatrixDouble(int rows, int cols)
        {
            Rows = rows;
            Columns = cols;

            _matrix = new List<List<double>>();

            for (int i = 0; i < rows; i++)
            {
                var vector = new List<double>();
                for (int j = 0; j < cols; j++)
                {
                    vector.Add(0);
                }
                _matrix.Add(vector);
            }
        }

        public int Rows { get; protected set; }
        public int Columns { get; protected set; }

        public double this[int row, int col]
        {
            get { return _matrix[row][col]; }
            set
            {
                if (row < Rows && col < Columns)
                    _matrix[row][col] = value;
            }
        }

        public static MatrixDouble Identity(int rows)
        {
            var ret = new MatrixDouble(rows, rows);

            for (int i = 0; i < rows; i++)
            {
                ret[i, i] = 1;
            }

            return ret;
        }

        public MatrixDouble Add(MatrixDouble other)
        {
            if (other.Rows != Rows || other.Columns != Columns) throw new Exception("Column/Row miss-match");

            var ret = new MatrixDouble(Rows, Columns);
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    ret[i, j] = this[i, j] + other[i, j];
                }
            }

            return ret;
        }

        public MatrixDouble Subtract(MatrixDouble other)
        {
            if (other.Rows != Rows || other.Columns != Columns) throw new Exception("Column/Row miss-match");

            var ret = new MatrixDouble(Rows, Columns);
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    ret[i, j] = this[i, j] - other[i, j];
                }
            }

            return ret;
        }

        public MatrixDouble Multiply(MatrixDouble other)
        {
            if (Columns != other.Rows) throw new Exception("Column/Row miss-match");

            var ret = new MatrixDouble(Rows, other.Columns);

            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < other.Columns; j++)
                {
                    double val = 0;
                    for (int k = 0; k < Columns; k++)
                    {
                        val += this[i, k] * other[k, j];
                    }
                    ret[i, j] = val;
                }
            }

            return ret;
        }

        public MatrixDouble Multiply(double value)
        {
            var ret = new MatrixDouble(Rows, Columns);
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    ret[i, j] = this[i, j] * value;
                }
            }

            return ret;
        }

        public MatrixDouble Transpose()
        {
            var ret = new MatrixDouble(Columns, Rows);
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    ret[j, i] = this[i, j];
                }
            }

            return ret;
        }

        public MatrixDouble Rotate()
        {
            var ret = new MatrixDouble(Columns, Rows);
            for (int i = 1; i <= Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    ret[j, Columns - i] = this[i - 1, j];
                }
            }

            return ret;
        }

        public MatrixDouble Normalize()
        {
            var ret = new MatrixDouble(Rows,Columns);
            double total = 0;

            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    total += this[i, j] * this[i, j];
                }
            }

            total = Math.Sqrt(total);

            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    ret[i,j] = this[i, j]/total;
                }
            }
            
            return ret;
        }


        public MatrixDouble Cofactors()
        {
            var ret = new MatrixDouble(Columns, Rows);
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    ret[i, j] = Cofactor(j, i) * ((i + j) % 2 == 0 ? 1 : -1);
                }
            }

            return ret;
        }

        public MatrixDouble Adjugate()
        {
            return Transpose().Cofactors();
        }

        public double Cofactor(int row, int col)
        {
            return RemoveColumn(col).RemoveRow(row).Determinant();
        }

        public double Determinant()
        {
            if (Rows != Columns) throw new Exception("Column/Row miss-match");

            if (Rows == 1)
                return this[0, 0];

            if (Rows == 2)
            {
                return (this[0, 0] * this[1, 1]) - (this[1, 0] * this[0, 1]);
            }

            if (Rows == 3)
            {
                return (this[0, 0] * this[1, 1] * this[2, 2]) +
                    (this[0, 1] * this[1, 2] * this[2, 0]) +
                    (this[0, 2] * this[1, 0] * this[2, 1]) -
                    (this[0, 2] * this[1, 1] * this[2, 0]) -
                    (this[0, 1] * this[1, 0] * this[2, 2]) -
                    (this[0, 0] * this[1, 2] * this[2, 1]);
            }

            double ret = 0;
            for (int i = 0; i < Rows; i++)
            {
                MatrixDouble temp = RemoveRow(0).RemoveColumn(i);
                ret += (i % 2 == 0 ? 1 : -1) * this[i, 0] * temp.Determinant();
            }
            return ret;
        }

        public MatrixDouble RemoveColumn(int col)
        {
            var ret = new MatrixDouble(Rows - 1, Columns);
            bool offset = false;
            for (int i = 0; i < Rows; i++)
            {
                if (col == i)
                {
                    offset = true;
                    continue;
                }

                for (int j = 0; j < Columns; j++)
                {
                    if (offset)
                        ret[i - 1, j] = this[i, j];
                    else
                        ret[i, j] = this[i, j];
                }
            }

            return ret;
        }

        public MatrixDouble RemoveRow(int row)
        {
            var ret = new MatrixDouble(Rows, Columns - 1);
            bool offset = false;
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    if (row == j)
                    {
                        offset = true;
                        continue;
                    }

                    if (offset)
                        ret[i, j - 1] = this[i, j];
                    else
                        ret[i, j] = this[i, j];
                }
                offset = false;
            }

            return ret;
        }

        public MatrixDouble Inverse()
        {
            var det = Determinant();
            return Adjugate().Multiply(1 / det);
        }

        public void Import(object array)
        {
            int index = 0;
            if (!(array is Array)) return;
            for (int i = 0; i < Columns; i++)
            {
                for (int j = 0; j < Rows; j++)
                {
                    this[j, i] = (double)((Array)array).GetValue(index++);
                }
            }
        }

        public void Import(List<double> array)
        {
            int index = 0;
            for (int i = 0; i < Columns; i++)
            {
                for (int j = 0; j < Rows; j++)
                {
                    this[j, i] = array[index++];
                }
            }
        }

        public MatrixDouble Clone()
        {
            return new MatrixDouble(Rows, Columns, ToArray());
        }

        public List<Double> ToArray()
        {
            var ret = new List<double>();
            for (int i = 0; i < Columns; i++)
            {
                for (int j = 0; j < Rows; j++)
                {
                    ret.Add(this[j, i]);
                }
            }
            return ret;
        }

        public bool IsEmpty()
        {
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Columns; j++)
                {
                    if (this[i, j] != 0)
                        return false;
                }
            }
            return true;
        }

        public override string ToString()
        {
            const string bracket = "{ ";
            const string closeBracket = "} ";
            string ret = bracket;

            for (int i = 0; i < Rows; i++)
            {
                ret += bracket;
                for (int j = 0; j < Columns; j++)
                {
                    ret += j == 0 ? this[i, j].ToString() : ", " + this[i, j];
                }
                ret += closeBracket;
            }
            ret += closeBracket;

            return ret;
        }
    }
}